package legway;
/**
 * A class which handles controlling the Legway directional movements. Works by altering
 * parameters in the CtrlParam class which are called by the BalanceController thread.
 * Original code by Steven Witzand
 * 
 * @author Kim Petersen, Andreas Koefoed-Hansen og Tim Rasmussen
 * @version Januar 2010
 */
public class Car
{
	private CtrlParam ctrl;
	private final double turnPower = 200; // Speed at which the Legway rotates
	private final double semiTurnBrake = 110; // How much it brakes one wheel when semi-turning

	/**
	 * Car constructor.
	 * 
	 * @param ctrl The motor control parameters.
	 */
	public Car(CtrlParam ctrl) {
		this.ctrl = ctrl;
	}

	/**
	 * Moves the Legway forward for a specified period of time.
	 * 
	 * @param period An integer number specifying the length to move the Legway forward.
	 */
	public void forward(int period) {
		ctrl.setLeftMotorOffset(0);
		ctrl.setRightMotorOffset(0);
		ctrl.setMaxSpeed(3,5);
		ctrl.setDriveState(CtrlParam.FORWARDS);
		for (int time = 0; time < period; time += 10) {
			try {
				Thread.sleep(10);
			} catch (InterruptedException e) { }
		}
	}

	/**
	 * Instruct Legway to drive forward until it is stopped by another command.
	 */
	public void forward() {
		ctrl.setLeftMotorOffset(0);
		ctrl.setRightMotorOffset(0);
		ctrl.setMaxSpeed(3,5);
		ctrl.setDriveState(CtrlParam.FORWARDS);
	}

	/**
	 * Instruct Legway to slowly drive forward until it is stopped by another command.
	 */
	public void slowForward() {
		ctrl.setLeftMotorOffset(0);
		ctrl.setRightMotorOffset(0);
		ctrl.setMaxSpeed(0.7,5);
		ctrl.setDriveState(CtrlParam.FORWARDS);
	}

	/**
	 * Instruct Legway to turn right while driving forward until it is stopped by another command.
	 */
	public void forwardRight() {
		forward();
		ctrl.setRightMotorOffset(-semiTurnBrake);
	}

	/**
	 * Instruct Legway to turn left while driving forward until it is stopped by another command.
	 */
	public void forwardLeft() {
		forward();
		ctrl.setLeftMotorOffset(-semiTurnBrake);
	}

	/**
	 * Moves the Legway backward for a specified period of time
	 * 
	 * @param period An integer number specifying the length to move the Legway backward.
	 */
	public void backward(int period) {
		ctrl.setLeftMotorOffset(0);
		ctrl.setRightMotorOffset(0);
		ctrl.setMaxSpeed(3,5);
		ctrl.setDriveState(CtrlParam.BACKWARDS);
		for (int time = 0; time < period; time += 10) {
			try {
				Thread.sleep(10);
			} catch (InterruptedException e) { }
		}
	}

	/**
	 * Instruct Legway to reverse until it is stopped by another command.
	 */
	public void backward() {
		ctrl.setLeftMotorOffset(0);
		ctrl.setRightMotorOffset(0);
		ctrl.setMaxSpeed(3,5);
		ctrl.setDriveState(CtrlParam.BACKWARDS);
	}

	/**
	 * Instruct Legway to turn right while reversing until it is stopped by another command.
	 */
	public void backwardRight() {
		backward();
		ctrl.setRightMotorOffset(semiTurnBrake);
	}

	/**
	 * Instruct Legway to turn left while reversing until it is stopped by another command.
	 */
	public void backwardLeft() {
		backward();
		ctrl.setLeftMotorOffset(semiTurnBrake);
	}

	/**
	 * Instruct Legway to turn right until it is stopped by another command.
	 * 
	 * @param period
	 *           An integer to delay other commands being sent to the Legway
	 */
	public void left(int period) {
		left();
		delay(period);
	}

	/**
	 * Instruct Legway to turn right until it is stopped by another command.
	 */
	public void left() {
		ctrl.setLeftMotorOffset(-turnPower);
		ctrl.setRightMotorOffset(turnPower);
	}

	/**
	 * Instruct Legway to turn right until it is stopped by another command.
	 * 
	 * @param period
	 *           An integer to delay other commands being sent to the Legway
	 */
	public void right(int period) {
		right();
		delay(period);
	}

	/**
	 * Instruct Legway to turn right until it is stopped by another command.
	 */
	public void right() {
		ctrl.setLeftMotorOffset(turnPower);
		ctrl.setRightMotorOffset(-turnPower);
	}

	/**
	 * Instruct Legway to stop, and stay stopped for a given period of time.
	 * 
	 * @param period An integer containing the number of millisecond to pause before returning.
	 */
	public void stop(int period) {
		stop();
		delay(period);
	}

	/**
	 * Instruct Legway to stop.
	 */
	public void stop() {
		ctrl.setLeftMotorOffset(0);
		ctrl.setRightMotorOffset(0);
		ctrl.resetDamp();
		ctrl.setDriveState(CtrlParam.STATIONARY);
	}

	/**
	 * Make the thread sleep for a specified period of time.
	 * 
	 * @param time An integer containing the number of milliseconds to sleep.
	 */
	public void delay(int time)	{
		try {
			Thread.sleep(time);
		} catch (Exception e) {}
	}
}
